/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.autonomous;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.input.AutoInputState;
import com.team1160.breakaway.model.RobotState;
import com.team1160.breakaway.model.robotCommand.RobotCommand;
import edu.wpi.first.wpilibj.DriverStation;

/**
 *
 * @author CJ
 */
public class AutonomousTaskManager {

    protected DriverStation.Alliance alliance;
    protected int location;

    protected int code;
    protected int tickTimes;

    protected long lastKickTime;
    protected long lastHoldTime;
    protected long lastKickReleaseTime;

    protected long autoStartTime;

    protected boolean kicking;

    protected boolean firstRun;
    protected long lastMoveTime;

    protected boolean autoDone;
    protected long autoDoneTime;
    
    public AutonomousTaskManager(){
        reset();
    }

    protected void reset(){
        this.firstRun = true;
        this.autoDone = false;
        this.alliance = DriverStation.getInstance().getAlliance();
        this.location = DriverStation.getInstance().getLocation();
        getCode();
    }

    protected void getCode(){
        if(this.alliance == DriverStation.Alliance.kRed){
            if(this.location == AutonomousPositions.FRONT_POSITION){
                this.code = AutonomousPositions.REDFRONT;
            }
            else if(this.location == AutonomousPositions.MIDDLE_POSITION){
                this.code = AutonomousPositions.REDMIDDLE;
            }
            else{
                this.code = AutonomousPositions.REDBACK;
            }
        }else{
            if(this.location == AutonomousPositions.FRONT_POSITION){
                this.code = AutonomousPositions.BLUEFRONT;
            }else if(this.location == AutonomousPositions.MIDDLE_POSITION){
                this.code = AutonomousPositions.BLUEMIDDLE;
            }else{
                this.code = AutonomousPositions.BLUEBACK;
            }
        }
    }

    public RobotCommand getAutonomousRobotCommand(RobotState robotState,
                AutoInputState inputState){
        //If someone changed the values of the DS change again
        if(DriverStation.getInstance().getAlliance() != this.alliance ||
                DriverStation.getInstance().getLocation() != this.location){
            reset();
        }

        //First Run, should only run once
        if(this.firstRun){

            if(inputState.getAutoSwitchOne() && inputState.getAutoSwitchTwo()){
                //Number 3
                this.tickTimes = 3;
            }else if(inputState.getAutoSwitchOne()){
                //Number 1
                this.tickTimes = 1;
            }else if(inputState.getAutoSwitchTwo()){
                //Number 2
                this.tickTimes = 2;
            }else{
                //Number 0
                this.tickTimes = 0;
            }

//            double inputLocation = inputState.getDribblerSpeed();
//            boolean plow = inputState.getDribblerOn();
//
//            if(plow)
//                this.tickTimes = 0;
//            else if(inputLocation < -0.8)
//                this.tickTimes = 3;
//            else if(inputLocation > -0.2)
//                this.tickTimes = 1;
//            else
//                this.tickTimes = 2;

            System.out.println("Number of balls to kick : " + this.tickTimes);

            this.firstRun = false;
            this.kicking = false;
            this.lastMoveTime = System.currentTimeMillis();
            this.autoStartTime = System.currentTimeMillis();
        }

        RobotCommand nextRobotCommand = new RobotCommand();
        nextRobotCommand.setShooter(robotState.isKickerKicking());
        nextRobotCommand.setHoldShooter(robotState.latched());
        if(tickTimes > 0){

            //Are we Kicking?
            if(this.kicking){
                //First step, we kick and are latched, must delay to kick
                if(robotState.isKickerKicking()
                        && robotState.latched()
                        && System.currentTimeMillis() - this.lastKickTime > Constants.DELAY_KICKER_RELEASE){
                    nextRobotCommand.setHoldShooter(false);
                    this.lastKickReleaseTime = System.currentTimeMillis();

                    //Second step, we are now fully extended and must delay to retract
                }else if(robotState.isKickerKicking()
                        && !robotState.latched()
                        && System.currentTimeMillis() - this.lastKickReleaseTime > Constants.DELAY_KICKER_RETRACT){
                    nextRobotCommand.setShooter(false);
                    this.lastHoldTime = System.currentTimeMillis();
                    
                    //Last step, we have retracted the foot and must delay to hold it in place
                    //Subtract the times we have to go again and reset some values
                }else if(!robotState.isKickerKicking()
                        && System.currentTimeMillis() - this.lastHoldTime > Constants.DELAY_KICKER_HOLDING){
                    nextRobotCommand.setHoldShooter(true);
                    --tickTimes;
                    if(tickTimes < 1){
                        autoDone = true;
                        this.autoDoneTime = System.currentTimeMillis();
                    }
                    this.kicking = false;
                    this.lastMoveTime = System.currentTimeMillis();
                }
            }
            
            if(System.currentTimeMillis() - this.lastMoveTime > 900 && !this.kicking){
                nextRobotCommand.setShooter(true);
                this.kicking = true;
                this.lastKickTime = System.currentTimeMillis();
                stopMoving(nextRobotCommand);
            }else if(this.kicking){
                nextRobotCommand.setDriveLeft(-Constants.SPEED_MAX_SAFETY);
                nextRobotCommand.setDriveRight(-Constants.SPEED_MAX_SAFETY);
            }else{
                nextRobotCommand.setLeftDrive(-Constants.SPEED_MAX_SAFETY);
                nextRobotCommand.setRightDrive(-Constants.SPEED_MAX_SAFETY);
            }
        }
        else if(!inputState.getAutoSwitchOne() &&
                !inputState.getAutoSwitchTwo() &&
                System.currentTimeMillis() - this.autoStartTime < 10000){
            nextRobotCommand.setDriveLeft(-0.5);
            nextRobotCommand.setDriveRight(-0.5);
        }else if(autoDone && System.currentTimeMillis() - this.autoDoneTime < 3000){
            nextRobotCommand.setDriveLeft(0.5);
            nextRobotCommand.setDriveRight(0.5);
        }else{
            stopMoving(nextRobotCommand);
        }
        return nextRobotCommand;
    }

    protected void stopMoving(RobotCommand nextRobotCommand){
        nextRobotCommand.setLeftDrive(0.0);
        nextRobotCommand.setRightDrive(0.0);
    }
}